/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 代价地图主类文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#include <costmap_2d/footprint.h>
#include <costmap_2d/layered_costmap.h>

#include <algorithm>
#include <cstdio>
#include <iostream>
#include <string>
#include <vector>

using std::vector;

namespace costmap_2d {

LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window,
                               bool track_unknown)
    : costmap_(),
      global_frame_(global_frame),
      rolling_window_(rolling_window),
      current_(false),
      minx_(0.0),
      miny_(0.0),
      maxx_(0.0),
      maxy_(0.0),
      bx0_(0),
      bxn_(0),
      by0_(0),
      byn_(0),
      initialized_(false),
      circumscribed_radius_(1.0),
      inscribed_radius_(0.1) {
  if (track_unknown) {
    costmap_.setDefaultValue(255);
  } else {
    costmap_.setDefaultValue(0);
  }
}

LayeredCostmap::~LayeredCostmap() {
  while (plugins_.size() > 0) {
    plugins_.pop_back();
  }
}

void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y,
                               double resolution, double origin_x,
                               double origin_y) {
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
  costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin();
       plugin != plugins_.end(); ++plugin) {
    (*plugin)->matchSize();
  }
}

void LayeredCostmap::updateMap(double robot_x, double robot_y,
                               double robot_yaw) {
  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
  // implement thread unsafe updateBounds() functions.
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
  // if we're using a rolling buffer costmap... we need to update the origin
  // using the robot's position
  if (rolling_window_) {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

  if (plugins_.size() == 0) {
    return;
  }

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin();
       plugin != plugins_.end(); ++plugin) {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_,
                            &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx ||
        maxy_ < prev_maxy) {
      std::cout << "Something happened when update Bounds !!!" << std::endl;
    }
  }

  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  if (xn < x0 || yn < y0) {
    return;
  }

  costmap_.resetMap(x0, y0, xn, yn);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin();
       plugin != plugins_.end(); ++plugin) {
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  initialized_ = true;
}

bool LayeredCostmap::isCurrent() {
  current_ = true;
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin();
       plugin != plugins_.end(); ++plugin) {
    current_ = current_ && (*plugin)->isCurrent();
  }
  return current_;
}

void LayeredCostmap::setFootprint(
    const std::vector<costmap_2d::Point>& footprint_spec) {
  footprint_ = footprint_spec;
  costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_,
                                          circumscribed_radius_);

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin();
       plugin != plugins_.end(); ++plugin) {
    (*plugin)->onFootprintChanged();
  }
}

}  // namespace costmap_2d
